{
 "cells": [
  {
   "cell_type": "markdown",
   "id": "1be9e839",
   "metadata": {
    "colab_type": "text",
    "id": "EgiF12Hf1Dhs"
   },
   "source": [
    "This notebook provides examples to go along with the [textbook](http://manipulation.csail.mit.edu/pick.html).  I recommend having both windows open, side-by-side!"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "b7e47535",
   "metadata": {
    "colab": {},
    "colab_type": "code",
    "id": "eeMrMI0-1Dhu"
   },
   "outputs": [],
   "source": [
    "from pydrake.all import MultibodyPlant, Parser"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "381e63bc",
   "metadata": {
    "colab_type": "text",
    "id": "7q0A14bAilIX"
   },
   "source": [
    "# Don't assume $\\dot{q} \\equiv v$\n",
    "\n",
    "Let's just add a single object into the scene.  We won't weld it to the world frame, so it is a \"free body\" or has a \"floating base\"."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "379660bf",
   "metadata": {
    "colab": {},
    "colab_type": "code",
    "id": "ILYLouFTjv6e"
   },
   "outputs": [],
   "source": [
    "def num_positions_velocities_example():\n",
    "    plant = MultibodyPlant(time_step=0.0)\n",
    "    Parser(plant).AddModelsFromUrl(\n",
    "        \"package://drake/examples/manipulation_station/models/061_foam_brick.sdf\"\n",
    "    )\n",
    "    plant.Finalize()\n",
    "\n",
    "    context = plant.CreateDefaultContext()\n",
    "    print(context)\n",
    "\n",
    "    print(f\"plant.num_positions() = {plant.num_positions()}\")\n",
    "    print(f\"plant.num_velocities() = {plant.num_velocities()}\")\n",
    "    print(\"\")\n",
    "    print(\n",
    "        f\"Position names: {plant.GetPositionNames(add_model_instance_prefix=False)}\"\n",
    "    )\n",
    "    print(\n",
    "        f\"Velocity names: {plant.GetVelocityNames(add_model_instance_prefix=False)}\"\n",
    "    )\n",
    "\n",
    "\n",
    "num_positions_velocities_example()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "32ad75c4",
   "metadata": {},
   "source": [
    "Looking at the `Context` you can see that this system has 13 total state variables.  7 of them are positions, $q$; this is due to our pose representation using unit quaternions.  But only 6 of them are velocities, $v$; this is because a six-element spatial velocity provides a better (unconstrained) representation of the rate of change of the unit quaternion.  But clearly, if the length of the vectors don't even match, we do *not* have $\\dot{q} = v$."
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3.10.6 64-bit",
   "language": "python",
   "name": "python3"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}
